"""
This Code is a interface between Single Board Heater System(Nex Robotics) and Controller(Computer). I wrote it for one of my course project.
"""


from pylab import* 
import serial 
import time
import numpy
import os,sys,getopt

sys.argv[1:]
try:                                
    opts, args = getopt.getopt(sys.argv[1:], "t:Kp:Kd:Ki:initial:step:") 
except getopt.GetoptError:
    usage()                          
    sys.exit(2)
tf = 200.0
Kp = 1.0
Ki = 0.0
Kd = 0.0
intial = 30.0
step = 5.0
for opt, arg in opts:               
    if opt in ("-t"): 
        tf = float(arg)               
    elif opt in ("Kp"):
        Kp = float(arg)
    elif opt in ("Kd"):
        Kd = float(arg)
    elif opt in ("Ki"):
        Ki = float(arg)
    elif opt in ("initial"):
        intial = float(arg)
    elif opt in ("step"):
       step = float(arg)


class SBHS:
    """Base class which contains everthing you need to control temperature 
    of SBHS. Our task was to use Heater input and keep fan at constant speed.
    Therefore we have kept fan at a constant value.

    To run in ipython
    import main
    a = main.SBHS()

    for Modelling
    a.modelling_run(time)
    
    for Control
    a.control_run(time)
    
    you can change the parameters in the code. I will make it mode user friendly
    when i will get time. As of now its working. And giving correct results!
    """

    def __init__(self):
        #Serial interface
        Port = '/dev/ttyUSB0'
        Baud = 9600
        Timeout = 1
        self.ser = serial.Serial(Port,Baud,timeout=Timeout)
        
        self.sensor_temp = 30.0
        self.set_temp = 30.0
        
        self.Kp = 8.0
        self.Ki = 6.0
        self.Kd = 0.0
        
        self.dt = .0007
        #PID variable
        self.prev_error = 0.0
        self.integral = 0.0
        self.controlout = 0.0
        self.t = 0.0
        #SBHS parameter
        self.fan = 100
        self.heater = 25

    def pid_action(self):
        
        Kp = self.Kp
        Ki = self.Ki
        Kd = self.Kd
        dt = self.dt

        error = self.set_temp-self.sensor_temp
        self.integral += error*dt
        derivative = (error - self.prev_error)/dt
        self.controlout = Kp*error + Ki*self.integral + Kd*derivative

        if self.controlout >= 0.0:
            self.heater = int(self.controlout)
        else:
            self.heater = 0
        
                

    def send_msg(self):
        ser = self.ser
        ser.write(chr(253))
        ser.write(chr(self.fan))
        ser.write(chr(254))
        ser.write(chr(self.heater))
        ser.write(chr(255))
        
    def read_msg(self):
        ser = self.ser
        tmp1 = ser.read()
        tmp2 = ser.read()
       
        self.sensor_temp = ord(tmp1)+ord(tmp2)*.1
                
    def control_run(self,tf):
        ti = time.time()
        self.t = ti
        t_input = []
        t_output = []
        t_time = []
        count = 0

        while tf > self.t:
            self.t = time.time() - ti
            self.send_msg()
            self.pid_action()
            self.read_msg()
            
            if self.t > tf/2:
                self.set_temp = 35.0
                if count == 0:
                    self.integral = 0.0
                    count = 1

            t_time.append(self.t)
            t_input.append(self.set_temp)
            t_output.append(self.sensor_temp)
            self.Print()

        tt = numpy.array(t_time)
        tti = numpy.array(t_input)
        tto = numpy.array(t_output)
        numpy.savez('control_run_'+str(tf)+'Kp'+str(self.Kp)+ \
                        'Ki'+str(self.Ki)+'Kd'+str(self.Kd)+'.npz',
                    t=tt,o=tto,i=tti)
        plot(tt,tti)
        plot(tt,tto)
        show()

    def modelling_run(self,tf):
        ti = time.time()
        t_input = []
        t_output = []
        t_time = []
        self.t = ti
        while tf > self.t:
            self.t = time.time() - ti
            self.send_msg()
            self.read_msg()
            
            if self.t > tf/2:
                self.heater = int(40.0)
            t_time.append(time.time()-ti)
            t_input.append(self.set_temp)
            t_output.append(self.sensor_temp)
            self.Print()

        tt = numpy.array(t_time)
        tti = numpy.array(t_input)
        tto = numpy.array(t_output)
        numpy.savez('modelling_run_'+str(tf)+'.npz',t=tt,o=tto,i=tti)
    
    def Print(self):
        print 'Time ', self.t,'  Control Effort ', self.controlout, 
        '  Temp ',self.sensor_temp
        
s = SBHS()
s.Kp = Kp
s.Ki = Ki
s.Kd = Kd
s.set_temp = initial
s.control_run(tf)
        
    








